copp\copp\copp3\opt3/
topp3_socp.rs

1//! 3rd-order Time-Optimal Path Parameterization (TOPP3) based on second-order cone programming (SOCP).
2//!
3//! # Method identity
4//! This module implements the **optimization backend** for TOPP3-QP by transforming
5//! third-order path-parameterization constraints/objective into Clarabel-compatible
6//! conic form and solving with SOCP.
7//!
8//! # Discrete variables (local notation)
9//! On a path grid `s[0..=n]`:
10//! - `a[k]` denotes $\dot{s}_k^2$;
11//! - `b[k]` denotes $\ddot{s}_k$;
12//! - auxiliary variables `xi[k]` and `eta[k]` satisfy reciprocal-SOC coupling for
13//!   the time objective in QP form.
14//! - decision vector is organized as
15//!   `x = [a[0..=n], b[0..=n], xi[0..len_xi), eta[0..len_xi)]`.
16//!
17//! # High-level pipeline
18//! 1. Validate boundary/index contracts.
19//! 2. Assemble standard TOPP3 conic constraints.
20//! 3. Add QP-specific SOC constraints for `(xi, eta)` and reciprocal coupling.
21//! 4. Build sparse matrices `A`, `P`, vector `q`, and solve by Clarabel.
22//! 5. Apply status acceptance policy ([`ClarabelOptions::is_allow`](crate::solver::copp2_socp::ClarabelOptions::is_allow)) and extract
23//!    a [`Topp3Profile`](crate::solver::topp3_socp::Topp3Profile) only when accepted.
24//!
25//! # API layering
26//! - [`topp3_socp`](crate::solver::topp3_socp::topp3_socp): strict/normal API, returns only accepted [`Topp3Profile`](crate::solver::topp3_socp::Topp3Profile).
27//! - [`topp3_socp_expert`](crate::solver::topp3_socp::topp3_socp_expert): expert API returning `(Option<Topp3Profile>, DefaultSolution<f64>)`.
28//! - [`topp3_socp_expert_with_info`](crate::solver::topp3_socp::topp3_socp_expert_with_info): expert API plus Clarabel linear-solver
29//!   metadata for wrappers that need solver-side diagnostics.
30
31use crate::copp::clarabel_backend::ConstraintsClarabel;
32use crate::copp::copp3::Topp3Profile;
33use crate::copp::copp3::formulation::{Topp3Problem, get_weight_a_topp3};
34use crate::copp::copp3::opt3::ClarabelExpertInfor3rd;
35use crate::copp::copp3::opt3::clarabel_constraints::{
36    clarabel_standard_capacity_topp3, clarabel_standard_constraint_topp3,
37};
38use crate::copp::{ClarabelOptions, clarabel_to_copp3_solution};
39use crate::diag::{
40    CoppError, DebugVerboser, SilentVerboser, SummaryVerboser, TraceVerboser, Verboser, Verbosity,
41    check_boundary_state_copp3_valid, check_s_interval_valid, format_duration_human,
42};
43use clarabel::algebra::CscMatrix;
44use clarabel::solver::SupportedConeT::{NonnegativeConeT, SecondOrderConeT};
45use clarabel::solver::{DefaultSolution, DefaultSolver, IPSolver, SupportedConeT};
46
47/// Strict TOPP3-SOCP API for production use.
48///
49/// # Purpose
50/// Use this entry when caller only needs a valid [`Topp3Profile`](crate::solver::topp3_socp::Topp3Profile) and treats
51/// non-accepted solver statuses as hard failures.
52///
53/// # Contract
54/// - Internally calls [`topp3_socp_expert`](crate::solver::topp3_socp::topp3_socp_expert).
55/// - Returns `Ok(Topp3Profile { .. })` **iff** `options.is_allow(solution.status)` is `true`.
56/// - Returns `Err(CoppError::ClarabelSolverStatus(...))` when status is not accepted.
57///
58/// # Returns
59/// Returns accepted TOPP3 profile.
60///
61/// # Errors
62/// Returns [`CoppError`](crate::diag::CoppError) on conic-model/solver failures and non-accepted solver status.
63///
64/// More details are provided in the documentation of [`topp3_socp_expert`](crate::solver::topp3_socp::topp3_socp_expert).
65pub fn topp3_socp(
66    problem: &Topp3Problem,
67    options: &ClarabelOptions,
68) -> Result<Topp3Profile, CoppError> {
69    let (result, solution) = topp3_socp_expert(problem, options)?;
70    result.ok_or_else(|| CoppError::ClarabelSolverStatus("topp3_socp".into(), solution.status))
71}
72
73/// Expert TOPP3-SOCP API with full Clarabel solution exposure.
74///
75/// # Return contract
76/// - `Ok((Some(result), solution))`: status accepted by `options.is_allow(solution.status)`.
77/// - `Ok((None, solution))`: solve finished but status not accepted.
78/// - `Err(...)`: input/model/solver-construction runtime failures.
79///
80/// # Returns
81/// Returns tuple `(Option<Topp3Profile>, DefaultSolution<f64>)` for diagnostic pipelines.
82///
83/// # Errors
84/// Returns [`CoppError`](crate::diag::CoppError) only for true build/runtime failures.
85///
86/// # Contract
87/// - caller handles `None` profile when status is not accepted;
88/// - acceptance policy is controlled by `options.is_allow`.
89///   See [`ClarabelOptions::is_allow`](crate::solver::topp3_socp::ClarabelOptions::is_allow)
90///   for a status-handling example.
91///
92/// # Verbosity behavior
93/// Logging is layered by `options.verbosity()`:
94/// - [`Silent`](Verbosity::Silent): no algorithm logs;
95/// - [`Summary`](Verbosity::Summary): lifecycle milestones and elapsed time;
96/// - [`Debug`](Verbosity::Debug): assembly-level counters and stage summaries;
97/// - [`Trace`](Verbosity::Trace): fine-grained stage deltas and solver snapshot diagnostics.
98pub fn topp3_socp_expert(
99    problem: &Topp3Problem,
100    options: &ClarabelOptions,
101) -> Result<(Option<Topp3Profile>, DefaultSolution<f64>), CoppError> {
102    let info = topp3_socp_expert_with_info(problem, options)?;
103    let _ = &info.linsolver;
104    Ok((info.result, info.solution))
105}
106
107/// Expert TOPP3-SOCP API with Clarabel solution and linear-solver diagnostics.
108///
109/// Use this variant when callers need more than
110/// [`DefaultSolution`](clarabel::solver::DefaultSolution), because Clarabel stores linear-solver metadata on the
111/// solver `info` object rather than inside the returned solution.
112///
113/// Status acceptance follows
114/// [`ClarabelOptions::is_allow`](crate::solver::topp3_socp::ClarabelOptions::is_allow);
115/// see that method for the shared status-handling pattern.
116pub fn topp3_socp_expert_with_info(
117    problem: &Topp3Problem,
118    options: &ClarabelOptions,
119) -> Result<ClarabelExpertInfor3rd, CoppError> {
120    match options.verbosity() {
121        Verbosity::Silent => topp3_socp_core(problem, (options, SilentVerboser)),
122        Verbosity::Summary => topp3_socp_core(problem, (options, SummaryVerboser::new())),
123        Verbosity::Debug => topp3_socp_core(problem, (options, DebugVerboser::new())),
124        Verbosity::Trace => topp3_socp_core(problem, (options, TraceVerboser::new())),
125    }
126}
127
128/// Core implementation for TOPP3-SOCP expert flow.
129///
130/// # Internal contract
131/// `options_verboser` packs:
132/// - `options`: acceptance policy and Clarabel numerical settings;
133/// - `verboser`: concrete logger implementation chosen by external verbosity dispatch.
134///
135/// # Invariants
136/// - decision-variable layout always starts with contiguous `a[0..=n]` and `b[0..=n]`;
137/// - auxiliary block `[xi, eta]` has shared length `length_xi_eta(n, num_stationary)`;
138/// - extracted `(a,b)` is produced only through [`clarabel_to_copp3_solution`](crate::solver::copp3_socp::clarabel_to_copp3_solution) when status is accepted.
139fn topp3_socp_core(
140    problem: &Topp3Problem,
141    options_verboser: (&ClarabelOptions, impl Verboser),
142) -> Result<ClarabelExpertInfor3rd, CoppError> {
143    let (options, mut verboser) = options_verboser;
144    let idx_s_start = problem.idx_s_start;
145    let a_boundary = problem.a_boundary;
146    let b_boundary = problem.b_boundary;
147    let num_stationary = problem.num_stationary;
148    if verboser.is_enabled(Verbosity::Summary) {
149        verboser.record_start_time();
150    }
151    if verboser.is_enabled(Verbosity::Trace) {
152        let settings = options.clarabel_settings();
153        crate::verbosity_log!(
154            crate::diag::Verbosity::Summary,
155            "topp3_socp: options snapshot -> allow(almost={}, max_iter={}, max_time={}, callback_term={}, insufficient_progress={}), tol_gap_rel={}, tol_feas={}, max_iter={}, verbose={}",
156            options.is_allow(clarabel::solver::SolverStatus::AlmostSolved),
157            options.is_allow(clarabel::solver::SolverStatus::MaxIterations),
158            options.is_allow(clarabel::solver::SolverStatus::MaxTime),
159            options.is_allow(clarabel::solver::SolverStatus::CallbackTerminated),
160            options.is_allow(clarabel::solver::SolverStatus::InsufficientProgress),
161            settings.tol_gap_rel,
162            settings.tol_feas,
163            settings.max_iter,
164            settings.verbose
165        );
166    }
167
168    // Check input validity
169    check_boundary_state_copp3_valid(a_boundary, b_boundary)?;
170    let n = problem.a_linearization.len() - 1;
171    let idx_s_final = idx_s_start + n;
172    if verboser.is_enabled(Verbosity::Summary) {
173        crate::verbosity_log!(
174            crate::diag::Verbosity::Summary,
175            "\ntopp3_socp started: {} <= idx_s <= {}, s_len = {}, num_stationary={:?}.",
176            idx_s_start,
177            idx_s_final,
178            problem.a_linearization.len(),
179            num_stationary
180        );
181    }
182    check_s_interval_valid("topp3_socp", idx_s_start, idx_s_final)?;
183    let len_xi = length_xi_eta(n, num_stationary);
184    let id_xi_start = 2 * (n + 1);
185    let id_eta_start = id_xi_start + len_xi;
186    // Let x = [a[0,1,...,n],
187    //          b[0,1,...,n],
188    //          xi[0,1,...,len_xi-1],
189    //          eta[0,1,...,len_xi-1]]
190    //       \in R^{2*(n+1)+2*len_xi}.
191    // Step 1. Deal with constraints
192    // s=b-A*x \in cone, where A[row[i],col[i]]=val[i], A \in R^{m*(n+1)}, b \in R^m, s \in R^m
193    // -s=-b+A*x
194    // Step 1.1 create constraints
195    let (cap_val_lp, cap_b_lp, cap_cone_lp) =
196        clarabel_standard_capacity_topp3(problem.constraints, (idx_s_start, idx_s_final));
197    let (cap_val_qp, cap_b_qp, cap_cone_qp) = clarabel_capacity_topp3_qp(n);
198    if verboser.is_enabled(Verbosity::Debug) {
199        crate::verbosity_log!(
200            crate::diag::Verbosity::Summary,
201            "topp3_socp: capacity estimate lp(val={cap_val_lp}, b={cap_b_lp}, cone={cap_cone_lp}), qp(val={cap_val_qp}, b={cap_b_qp}, cone={cap_cone_qp}), n_var={}",
202            id_eta_start + len_xi
203        );
204    }
205    let mut cones = Vec::<SupportedConeT<f64>>::with_capacity(cap_cone_lp + cap_cone_qp);
206    let mut row = Vec::<usize>::with_capacity(cap_val_lp + cap_val_qp);
207    let mut col = Vec::<usize>::with_capacity(cap_val_lp + cap_val_qp);
208    let mut val = Vec::<f64>::with_capacity(cap_val_lp + cap_val_qp);
209    let mut b = Vec::<f64>::with_capacity(cap_b_lp + cap_b_qp);
210    if verboser.is_enabled(Verbosity::Trace) {
211        crate::verbosity_log!(
212            crate::diag::Verbosity::Summary,
213            "topp3_socp: allocated capacities row/col/val/b/cones <= {}/{}/{}/{}/{}",
214            cap_val_lp + cap_val_qp,
215            cap_val_lp + cap_val_qp,
216            cap_val_lp + cap_val_qp,
217            cap_b_lp + cap_b_qp,
218            cap_cone_lp + cap_cone_qp
219        );
220    }
221
222    // Step 1.2 deal with standard constraints
223    let s = problem.constraints.s_vec(idx_s_start, idx_s_final + 1)?;
224    let row_before_std = row.len();
225    let col_before_std = col.len();
226    let val_before_std = val.len();
227    let b_before_std = b.len();
228    let cones_before_std = cones.len();
229    clarabel_standard_constraint_topp3(
230        problem,
231        &s,
232        (&mut row, &mut col, &mut val, &mut b, &mut cones),
233        num_stationary,
234        &verboser,
235    )?;
236    if verboser.is_enabled(Verbosity::Trace) {
237        crate::verbosity_log!(
238            crate::diag::Verbosity::Summary,
239            "topp3_socp: standard-constraints delta row/col/val/b/cones = +{}/+{}/+{}/+{}/+{}",
240            row.len() - row_before_std,
241            col.len() - col_before_std,
242            val.len() - val_before_std,
243            b.len() - b_before_std,
244            cones.len() - cones_before_std
245        );
246    }
247    // Step 1.3 deal with additional constraints for QP
248    let row_before_qp = row.len();
249    let col_before_qp = col.len();
250    let val_before_qp = val.len();
251    let b_before_qp = b.len();
252    let cones_before_qp = cones.len();
253    clarabel_constraint_topp3_qp(
254        (&mut row, &mut col, &mut val, &mut b, &mut cones),
255        (idx_s_start, idx_s_final),
256        num_stationary,
257        id_xi_start,
258        id_eta_start,
259    );
260    if verboser.is_enabled(Verbosity::Trace) {
261        crate::verbosity_log!(
262            crate::diag::Verbosity::Summary,
263            "topp3_socp: qp-aux delta row/col/val/b/cones = +{}/+{}/+{}/+{}/+{}",
264            row.len() - row_before_qp,
265            col.len() - col_before_qp,
266            val.len() - val_before_qp,
267            b.len() - b_before_qp,
268            cones.len() - cones_before_qp
269        );
270    }
271
272    // Step 1.4 build the constraints
273    let n_var = id_eta_start + len_xi;
274    let row_len = row.len();
275    let col_len = col.len();
276    let val_len = val.len();
277    let b_len = b.len();
278    let cones_len = cones.len();
279    let a_csc = CscMatrix::new_from_triplets(b.len(), n_var, row, col, val);
280    // Step 2. objective function (time QP surrogate): min \sum w[k] * eta[k]
281    let p_object = CscMatrix::<f64>::zeros((n_var, n_var));
282    let q_object = clarabel_q_object_topp3_qp(&s, num_stationary, n_var, id_eta_start);
283    if verboser.is_enabled(Verbosity::Trace) {
284        let (q_min, q_max) = q_object
285            .iter()
286            .fold((f64::INFINITY, f64::NEG_INFINITY), |(mn, mx), &v| {
287                (mn.min(v), mx.max(v))
288            });
289        crate::verbosity_log!(
290            crate::diag::Verbosity::Summary,
291            "topp3_socp: matrix built with m={}, n={}, A.nnz={}, P.nnz={}, q_range=[{}, {}]",
292            b_len,
293            n_var,
294            a_csc.nnz(),
295            p_object.nnz(),
296            q_min,
297            q_max
298        );
299    }
300    if verboser.is_enabled(Verbosity::Summary) {
301        crate::verbosity_log!(
302            crate::diag::Verbosity::Summary,
303            "topp3_socp: ready to solve with row/col/val/b/cones = {row_len}/{col_len}/{val_len}/{b_len}/{cones_len} and n_var = {n_var}.",
304        );
305    }
306    // Step 3. solve the SOCP problem
307    let settings = options.clarabel_settings().clone();
308    let mut solver = DefaultSolver::<f64>::new(&p_object, &q_object, &a_csc, &b, &cones, settings)
309        .map_err(|e| CoppError::ClarabelSolverError("topp3_socp".into(), e))?;
310    solver.solve();
311    let linsolver = solver.info.linsolver.clone();
312    let solution = solver.solution;
313    if verboser.is_enabled(Verbosity::Summary) {
314        crate::verbosity_log!(
315            crate::diag::Verbosity::Summary,
316            "topp3_socp: solve done, status = {:?}, elapsed = {}.",
317            solution.status,
318            format_duration_human(verboser.elapsed())
319        );
320    }
321    if verboser.is_enabled(Verbosity::Trace) {
322        let show = solution.x.len().min(3);
323        crate::verbosity_log!(
324            crate::diag::Verbosity::Summary,
325            "topp3_socp: solution x_len={}, head={:?}",
326            solution.x.len(),
327            &solution.x[0..show]
328        );
329    }
330    let result = if options.is_allow(solution.status) {
331        Some(clarabel_to_copp3_solution(
332            &solution.x.as_slice()[0..2 * (n + 1)],
333            &s,
334            num_stationary,
335        ))
336    } else {
337        None
338    };
339    if verboser.is_enabled(Verbosity::Trace) {
340        crate::verbosity_log!(
341            crate::diag::Verbosity::Summary,
342            "topp3_socp: allow(status)={}, extracted_profile={}",
343            options.is_allow(solution.status),
344            if result.is_some() {
345                "Some(Topp3Profile)"
346            } else {
347                "None"
348            }
349        );
350    }
351    Ok(ClarabelExpertInfor3rd {
352        result,
353        solution,
354        linsolver,
355    })
356}
357
358/// Determine the length of `xi` and `eta` in the decision variable `x`.
359#[inline(always)]
360fn length_xi_eta(n: usize, num_stationary: (usize, usize)) -> usize {
361    n + 1 - num_stationary.0.max(1) - num_stationary.1.max(1)
362}
363
364/// Return `k_skip`, where `eta[k] = 1/sqrt(a[k + k_skip])`.
365#[inline(always)]
366fn skip_a_for_xi(num_stationary_start: usize) -> usize {
367    num_stationary_start.max(1)
368}
369
370/// Create the constraints for clarabel TOPP3-QP.
371/// `idx_s_interval`: (idx_s_start, idx_s_final), the interval of s for which we want to compute the time-optimal profile.
372/// `num_stationary`: (num_stationary_start, num_stationary_final), the number of stationary points at the start and final of the interval.
373/// `id_xi_start`: the starting index of xi in the decision variable x.
374/// `id_eta_start`: the starting index of eta in the decision variable x.
375fn clarabel_constraint_topp3_qp(
376    constraints: ConstraintsClarabel,
377    idx_s_interval: (usize, usize),
378    num_stationary: (usize, usize),
379    id_xi_start: usize,
380    id_eta_start: usize,
381) {
382    let (idx_s_start, idx_s_final) = idx_s_interval;
383    let n = idx_s_final - idx_s_start;
384    // s=b-A*x \in cone, where A[row[i],col[i]]=val[i]
385    // -s=-b+A*x
386    let (row, col, val, b, cones) = constraints;
387    // Add constraints for xi and eta
388    // xi[k] >= 0, eta[k] >= 0
389    // norm2([2, xi[k] - eta[k]]) <= xi[k] + eta[k]
390    // xi[k] * xi[k] <= a[k + k_skip]
391    let len_xi = length_xi_eta(n, num_stationary);
392    let k_skip = skip_a_for_xi(num_stationary.0);
393    // Step 1. xi[i] >= 0
394    // A*x-b = -s = -1*xi[k] <= 0
395    row.extend(b.len()..(b.len() + len_xi));
396    col.extend(id_xi_start..(id_xi_start + len_xi));
397    val.resize(val.len() + len_xi, -1.0);
398    b.resize(b.len() + len_xi, 0.0);
399    // Step 2. eta[i] >= 0
400    // A*x-b = -s = -1*eta[k] <= 0
401    row.extend(b.len()..(b.len() + len_xi));
402    col.extend(id_eta_start..(id_eta_start + len_xi));
403    val.resize(val.len() + len_xi, -1.0);
404    b.resize(b.len() + len_xi, 0.0);
405    cones.push(NonnegativeConeT(2 * len_xi));
406    // Step 3. norm2([2, xi[k] - eta[k]]) <= xi[k] + eta[k]
407    // -A*x+b = s = [xi[k] + eta[k], xi[k] - eta[k], 2] \in SOC
408    for k in 0..len_xi {
409        // xi[k] + eta[k]
410        row.resize(row.len() + 2, b.len());
411        col.extend([id_xi_start + k, id_eta_start + k]);
412        val.extend([-1.0, -1.0]);
413        b.push(0.0);
414        // xi[k] - eta[k]
415        row.resize(row.len() + 2, b.len());
416        col.extend([id_xi_start + k, id_eta_start + k]);
417        val.extend([-1.0, 1.0]);
418        b.push(0.0);
419        // 2
420        b.push(2.0);
421    }
422    // Step 4. xi[k] * xi[k] <= a[k_skip + k]
423    // norm2([2*xi[k], a[k_skip + k] - 1]) <= a[k_skip + k] + 1
424    // -A*x+b = s = [a[k_skip + k] + 1, a[k_skip + k] - 1, 2*xi[k]] \in SOC
425    for k in 0..len_xi {
426        // a[k_skip + k] + 1
427        row.push(b.len());
428        col.push(k_skip + k);
429        val.push(-1.0);
430        b.push(1.0);
431        // a[k_skip + k] - 1
432        row.push(b.len());
433        col.push(k_skip + k);
434        val.push(-1.0);
435        b.push(-1.0);
436        // 2*xi[k]
437        row.push(b.len());
438        col.push(id_xi_start + k);
439        val.push(-2.0);
440        b.push(0.0);
441    }
442    cones.resize(cones.len() + 2 * len_xi, SecondOrderConeT(3));
443}
444
445/// Build the linear objective coefficient `q` for TOPP3-QP.
446#[inline(always)]
447fn clarabel_q_object_topp3_qp(
448    s: &[f64],
449    num_stationary: (usize, usize),
450    n_var: usize,
451    id_eta_start: usize,
452) -> Vec<f64> {
453    let mut q_object = Vec::<f64>::with_capacity(n_var);
454    let weight = get_weight_a_topp3(s, num_stationary);
455    let len_eta = length_xi_eta(s.len() - 1, num_stationary);
456    let k_skip = skip_a_for_xi(num_stationary.0);
457    q_object.resize(id_eta_start, 0.0);
458    q_object.extend(weight[k_skip..(k_skip + len_eta)].iter());
459    q_object.resize(n_var, 0.0);
460    q_object
461}
462
463/// Determine Clarabel pre-allocation capacity for TOPP3-QP auxiliary constraints.
464///
465/// Returns `(capacity_val, capacity_b, capacity_cones)` as upper bounds.
466#[inline(always)]
467fn clarabel_capacity_topp3_qp(n: usize) -> (usize, usize, usize) {
468    // Step 1. xi[k] >= 0, eta[k] >= 0
469    //         (num_val==2*len_xi; num_b==2*len_xi, num_cone==1)
470    // Step 2. [2, xi[k] - eta[k], xi[k] + eta[k]] \in SOC
471    //         (num_val==4*len_xi; num_b==3*len_xi, num_cone==len_xi)
472    // Step 3. [2*xi[k], a[num_stationary.0 + k] - 1, a[num_stationary.0 + k] + 1] \in SOC
473    //         (num_val==3*len_xi; num_b==3*len_xi, num_cone==len_xi)
474    // len_xi = n + 1 - num_stationary.0 - num_stationary.1 <= n + 1
475    let len_xi_upper_bound = n + 1;
476    (
477        9 * len_xi_upper_bound,
478        8 * len_xi_upper_bound,
479        2 * len_xi_upper_bound + 1,
480    )
481}
482
483#[cfg(test)]
484mod tests {
485    use super::*;
486    use crate::copp::copp2::stable::basic::{Topp2ProblemBuilder, s_to_t_topp2};
487    use crate::copp::copp2::stable::reach_set2::{ReachSet2Options, ReachSet2OptionsBuilder};
488    use crate::copp::copp2::stable::topp2_ra::topp2_ra;
489    use crate::copp::copp3::stable::basic::{Topp3ProblemBuilder, s_to_t_topp3};
490    use crate::copp::{ClarabelOptions, ClarabelOptionsBuilder};
491    use crate::path::{add_symmetric_axial_limits_for_test, lissajous_path_for_test};
492    use crate::robot::robot_core::Robot;
493    use crate::solver::topp3_lp::topp3_lp;
494    use core::f64;
495    use nalgebra::DMatrix;
496    use std::time::Instant;
497
498    #[test]
499    fn test_topp3_lp_qp() -> Result<(), CoppError> {
500        run_test_topp3_lp_qp_repeated(1, false)
501    }
502
503    /// Conditions: release, --include-ignored, CPU = Intel(R) Core(TM) Ultra 9 285K.
504    /// AAverage (fail 1): tc_ra = 0.3294 ms, tc_lp = 257.8678 ms, tc_qp = 327.9065 ms, tf_ra = 6.1838, tf_lp = 7.1079, tf_qp = 7.1079
505    #[test]
506    #[ignore = "slow"]
507    fn test_topp3_lp_qp_robust() -> Result<(), CoppError> {
508        run_test_topp3_lp_qp_repeated(100, true)
509    }
510
511    fn run_one_topp3_lp_qp_case(
512        options_ra: &ReachSet2Options,
513        options_lp: &ClarabelOptions,
514        options_qp: &ClarabelOptions,
515    ) -> Result<(f64, f64, f64, f64, f64, f64), CoppError> {
516        let n: usize = 1000;
517        let dim = 7;
518        let mut rng = rand::rng();
519        let (_s_uniform, path, _, _) =
520            lissajous_path_for_test(dim, n, &mut rng).expect("random range is valid");
521
522        let mut robot = Robot::with_capacity(dim, n);
523        let s = DMatrix::<f64>::from_fn(1, n, |_, j| {
524            (j as f64
525                + (if 0 < j && 2 * j < n { 0.5 } else { 0.0 }
526                    + if n > j && 2 * j > n { 0.5 } else { 0.0 })
527                    * j as f64
528                    / n as f64)
529                * (1.0 / (n - 1) as f64)
530        });
531        robot
532            .with_s(&s.as_view())?
533            .with_q_from_path_3rd(&path, 0, n)?;
534        add_symmetric_axial_limits_for_test(&mut robot, 1.0, 1.0, Some(5.0))?;
535
536        let topp2_problem = Topp2ProblemBuilder::new(&robot, (0, n - 1), (0.0, 0.0)).build()?;
537        let start = Instant::now();
538        let a_ra = topp2_ra(&topp2_problem, options_ra)?;
539        let tc_ra = start.elapsed().as_secs_f64() * 1E3;
540        let (tf_ra, _) = s_to_t_topp2(s.as_slice(), &a_ra, 0.0)?;
541
542        robot.constraints.amax_substitute(&a_ra, 0)?;
543        let topp3_problem = Topp3ProblemBuilder::new(&mut robot, 0, &a_ra, (0.0, 0.0), (0.0, 0.0))
544            .with_num_stationary_max(2)
545            .build_with_linearization()?;
546
547        let start = Instant::now();
548        let profile_lp = topp3_lp(&topp3_problem, options_lp)?;
549        let tc_lp = start.elapsed().as_secs_f64() * 1E3;
550        let (tf_lp, _) = s_to_t_topp3(s.as_slice(), profile_lp.as_parts(), 0.0)?;
551
552        let start = Instant::now();
553        let profile_qp = topp3_socp(&topp3_problem, options_qp)?;
554        let tc_qp = start.elapsed().as_secs_f64() * 1E3;
555        let (tf_qp, _) = s_to_t_topp3(s.as_slice(), profile_qp.as_parts(), 0.0)?;
556
557        Ok((tc_ra, tc_lp, tc_qp, tf_ra, tf_lp, tf_qp))
558    }
559
560    fn run_test_topp3_lp_qp_repeated(n_exp: usize, flag_print_step: bool) -> Result<(), CoppError> {
561        let options_ra = ReachSet2OptionsBuilder::new()
562            .lp_feas_tol(1E-9)
563            .a_cmp_abs_tol(1E-9)
564            .a_cmp_rel_tol(1E-9)
565            .build()?;
566        let options_lp = ClarabelOptionsBuilder::new()
567            .allow_almost_solved(true)
568            .build()?;
569        let options_qp = ClarabelOptionsBuilder::new()
570            .allow_almost_solved(true)
571            .build()?;
572
573        let mut tc_sum_ra = 0.0;
574        let mut tc_sum_lp = 0.0;
575        let mut tc_sum_qp = 0.0;
576        let mut tf_sum_ra = 0.0;
577        let mut tf_sum_lp = 0.0;
578        let mut tf_sum_qp = 0.0;
579        let mut succeed = 0;
580
581        for i_exp in 0..n_exp {
582            if let Ok((tc_ra, tc_lp, tc_qp, tf_ra, tf_lp, tf_qp)) =
583                run_one_topp3_lp_qp_case(&options_ra, &options_lp, &options_qp)
584            {
585                if flag_print_step {
586                    crate::verbosity_log!(
587                        crate::diag::Verbosity::Summary,
588                        "Exp #{}: tc_ra = {:.4} ms, tc_lp = {:.4} ms, tc_qp = {:.4} ms, tf_ra = {:.4}, tf_lp = {:.4}, tf_qp = {:.4}",
589                        i_exp + 1,
590                        tc_ra,
591                        tc_lp,
592                        tc_qp,
593                        tf_ra,
594                        tf_lp,
595                        tf_qp,
596                    );
597                }
598                tc_sum_ra += tc_ra;
599                tc_sum_lp += tc_lp;
600                tc_sum_qp += tc_qp;
601                tf_sum_ra += tf_ra;
602                tf_sum_lp += tf_lp;
603                tf_sum_qp += tf_qp;
604                succeed += 1;
605            }
606        }
607
608        crate::verbosity_log!(
609            crate::diag::Verbosity::Summary,
610            "Average (fail {}): tc_ra = {:.4} ms, tc_lp = {:.4} ms, tc_qp = {:.4} ms, tf_ra = {:.4}, tf_lp = {:.4}, tf_qp = {:.4}",
611            n_exp - succeed,
612            tc_sum_ra / succeed as f64,
613            tc_sum_lp / succeed as f64,
614            tc_sum_qp / succeed as f64,
615            tf_sum_ra / succeed as f64,
616            tf_sum_lp / succeed as f64,
617            tf_sum_qp / succeed as f64,
618        );
619
620        Ok(())
621    }
622}
copp\copp\copp3\opt3/topp3_socp.rs

copp\copp\copp3\opt3/
topp3_socp.rs
